// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2013 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
// Copyright (C) 2013 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_SPARSEBLOCKMATRIX_H
#define EIGEN_SPARSEBLOCKMATRIX_H

namespace Eigen {
/** \ingroup SparseCore_Module
 *
 * \class BlockSparseMatrix
 *
 * \brief A versatile sparse matrix representation where each element is a block
 *
 * This class provides routines to manipulate block sparse matrices stored in a
 * BSR-like representation. There are two main types :
 *
 * 1. All blocks have the same number of rows and columns, called block size
 * in the following. In this case, if this block size is known at compile time,
 * it can be given as a template parameter like
 * \code
 * BlockSparseMatrix<Scalar, 3, ColMajor> bmat(b_rows, b_cols);
 * \endcode
 * Here, bmat is a b_rows x b_cols block sparse matrix
 * where each coefficient is a 3x3 dense matrix.
 * If the block size is fixed but will be given at runtime,
 * \code
 * BlockSparseMatrix<Scalar, Dynamic, ColMajor> bmat(b_rows, b_cols);
 * bmat.setBlockSize(block_size);
 * \endcode
 *
 * 2. The second case is for variable-block sparse matrices.
 * Here each block has its own dimensions. The only restriction is that all the blocks
 * in a row (resp. a column) should have the same number of rows (resp. of columns).
 * It is thus required in this case to describe the layout of the matrix by calling
 * setBlockLayout(rowBlocks, colBlocks).
 *
 * In any of the previous case, the matrix can be filled by calling setFromTriplets().
 * A regular sparse matrix can be converted to a block sparse matrix and vice versa.
 * It is obviously required to describe the block layout beforehand by calling either
 * setBlockSize() for fixed-size blocks or setBlockLayout for variable-size blocks.
 *
 * \tparam _Scalar The Scalar type
 * \tparam _BlockAtCompileTime The block layout option. It takes the following values
 * Dynamic : block size known at runtime
 * a numeric number : fixed-size block known at compile time
 */
template<typename _Scalar, int _BlockAtCompileTime = Dynamic, int _Options = ColMajor, typename _StorageIndex = int>
class BlockSparseMatrix;

template<typename BlockSparseMatrixT>
class BlockSparseMatrixView;

namespace internal {
template<typename _Scalar, int _BlockAtCompileTime, int _Options, typename _Index>
struct traits<BlockSparseMatrix<_Scalar, _BlockAtCompileTime, _Options, _Index>>
{
	typedef _Scalar Scalar;
	typedef _Index Index;
	typedef Sparse StorageKind; // FIXME Where is it used ??
	typedef MatrixXpr XprKind;
	enum
	{
		RowsAtCompileTime = Dynamic,
		ColsAtCompileTime = Dynamic,
		MaxRowsAtCompileTime = Dynamic,
		MaxColsAtCompileTime = Dynamic,
		BlockSize = _BlockAtCompileTime,
		Flags = _Options | NestByRefBit | LvalueBit,
		CoeffReadCost = NumTraits<Scalar>::ReadCost,
		SupportedAccessPatterns = InnerRandomAccessPattern
	};
};
template<typename BlockSparseMatrixT>
struct traits<BlockSparseMatrixView<BlockSparseMatrixT>>
{
	typedef Ref<
		Matrix<typename BlockSparseMatrixT::Scalar, BlockSparseMatrixT::BlockSize, BlockSparseMatrixT::BlockSize>>
		Scalar;
	typedef Ref<
		Matrix<typename BlockSparseMatrixT::RealScalar, BlockSparseMatrixT::BlockSize, BlockSparseMatrixT::BlockSize>>
		RealScalar;
};

// Function object to sort a triplet list
template<typename Iterator, bool IsColMajor>
struct TripletComp
{
	typedef typename Iterator::value_type Triplet;
	bool operator()(const Triplet& a, const Triplet& b)
	{
		if (IsColMajor)
			return ((a.col() == b.col() && a.row() < b.row()) || (a.col() < b.col()));
		else
			return ((a.row() == b.row() && a.col() < b.col()) || (a.row() < b.row()));
	}
};
} // end namespace internal

/* Proxy to view the block sparse matrix as a regular sparse matrix */
template<typename BlockSparseMatrixT>
class BlockSparseMatrixView : public SparseMatrixBase<BlockSparseMatrixT>
{
  public:
	typedef Ref<typename BlockSparseMatrixT::BlockScalar> Scalar;
	typedef Ref<typename BlockSparseMatrixT::BlockRealScalar> RealScalar;
	typedef typename BlockSparseMatrixT::Index Index;
	typedef BlockSparseMatrixT Nested;
	enum
	{
		Flags = BlockSparseMatrixT::Options,
		Options = BlockSparseMatrixT::Options,
		RowsAtCompileTime = BlockSparseMatrixT::RowsAtCompileTime,
		ColsAtCompileTime = BlockSparseMatrixT::ColsAtCompileTime,
		MaxColsAtCompileTime = BlockSparseMatrixT::MaxColsAtCompileTime,
		MaxRowsAtCompileTime = BlockSparseMatrixT::MaxRowsAtCompileTime
	};

  public:
	BlockSparseMatrixView(const BlockSparseMatrixT& spblockmat)
		: m_spblockmat(spblockmat)
	{
	}

	Index outerSize() const { return (Flags & RowMajorBit) == 1 ? this->rows() : this->cols(); }
	Index cols() const { return m_spblockmat.blockCols(); }
	Index rows() const { return m_spblockmat.blockRows(); }
	Scalar coeff(Index row, Index col) { return m_spblockmat.coeff(row, col); }
	Scalar coeffRef(Index row, Index col) { return m_spblockmat.coeffRef(row, col); }
	// Wrapper to iterate over all blocks
	class InnerIterator : public BlockSparseMatrixT::BlockInnerIterator
	{
	  public:
		InnerIterator(const BlockSparseMatrixView& mat, Index outer)
			: BlockSparseMatrixT::BlockInnerIterator(mat.m_spblockmat, outer)
		{
		}
	};

  protected:
	const BlockSparseMatrixT& m_spblockmat;
};

// Proxy to view a regular vector as a block vector
template<typename BlockSparseMatrixT, typename VectorType>
class BlockVectorView
{
  public:
	enum
	{
		BlockSize = BlockSparseMatrixT::BlockSize,
		ColsAtCompileTime = VectorType::ColsAtCompileTime,
		RowsAtCompileTime = VectorType::RowsAtCompileTime,
		Flags = VectorType::Flags
	};
	typedef Ref<const Matrix<typename BlockSparseMatrixT::Scalar,
							 (RowsAtCompileTime == 1) ? 1 : BlockSize,
							 (ColsAtCompileTime == 1) ? 1 : BlockSize>>
		Scalar;
	typedef typename BlockSparseMatrixT::Index Index;

  public:
	BlockVectorView(const BlockSparseMatrixT& spblockmat, const VectorType& vec)
		: m_spblockmat(spblockmat)
		, m_vec(vec)
	{
	}
	inline Index cols() const { return m_vec.cols(); }
	inline Index size() const { return m_spblockmat.blockRows(); }
	inline Scalar coeff(Index bi) const
	{
		Index startRow = m_spblockmat.blockRowsIndex(bi);
		Index rowSize = m_spblockmat.blockRowsIndex(bi + 1) - startRow;
		return m_vec.middleRows(startRow, rowSize);
	}
	inline Scalar coeff(Index bi, Index j) const
	{
		Index startRow = m_spblockmat.blockRowsIndex(bi);
		Index rowSize = m_spblockmat.blockRowsIndex(bi + 1) - startRow;
		return m_vec.block(startRow, j, rowSize, 1);
	}

  protected:
	const BlockSparseMatrixT& m_spblockmat;
	const VectorType& m_vec;
};

template<typename VectorType, typename Index>
class BlockVectorReturn;

// Proxy to view a regular vector as a block vector
template<typename BlockSparseMatrixT, typename VectorType>
class BlockVectorReturn
{
  public:
	enum
	{
		ColsAtCompileTime = VectorType::ColsAtCompileTime,
		RowsAtCompileTime = VectorType::RowsAtCompileTime,
		Flags = VectorType::Flags
	};
	typedef Ref<Matrix<typename VectorType::Scalar, RowsAtCompileTime, ColsAtCompileTime>> Scalar;
	typedef typename BlockSparseMatrixT::Index Index;

  public:
	BlockVectorReturn(const BlockSparseMatrixT& spblockmat, VectorType& vec)
		: m_spblockmat(spblockmat)
		, m_vec(vec)
	{
	}
	inline Index size() const { return m_spblockmat.blockRows(); }
	inline Scalar coeffRef(Index bi)
	{
		Index startRow = m_spblockmat.blockRowsIndex(bi);
		Index rowSize = m_spblockmat.blockRowsIndex(bi + 1) - startRow;
		return m_vec.middleRows(startRow, rowSize);
	}
	inline Scalar coeffRef(Index bi, Index j)
	{
		Index startRow = m_spblockmat.blockRowsIndex(bi);
		Index rowSize = m_spblockmat.blockRowsIndex(bi + 1) - startRow;
		return m_vec.block(startRow, j, rowSize, 1);
	}

  protected:
	const BlockSparseMatrixT& m_spblockmat;
	VectorType& m_vec;
};

// Block version of the sparse dense product
template<typename Lhs, typename Rhs>
class BlockSparseTimeDenseProduct;

namespace internal {

template<typename BlockSparseMatrixT, typename VecType>
struct traits<BlockSparseTimeDenseProduct<BlockSparseMatrixT, VecType>>
{
	typedef Dense StorageKind;
	typedef MatrixXpr XprKind;
	typedef typename BlockSparseMatrixT::Scalar Scalar;
	typedef typename BlockSparseMatrixT::Index Index;
	enum
	{
		RowsAtCompileTime = Dynamic,
		ColsAtCompileTime = Dynamic,
		MaxRowsAtCompileTime = Dynamic,
		MaxColsAtCompileTime = Dynamic,
		Flags = 0,
		CoeffReadCost = internal::traits<BlockSparseMatrixT>::CoeffReadCost
	};
};
} // end namespace internal

template<typename Lhs, typename Rhs>
class BlockSparseTimeDenseProduct : public ProductBase<BlockSparseTimeDenseProduct<Lhs, Rhs>, Lhs, Rhs>
{
  public:
	EIGEN_PRODUCT_PUBLIC_INTERFACE(BlockSparseTimeDenseProduct)

	BlockSparseTimeDenseProduct(const Lhs& lhs, const Rhs& rhs)
		: Base(lhs, rhs)
	{
	}

	template<typename Dest>
	void scaleAndAddTo(Dest& dest, const typename Rhs::Scalar& alpha) const
	{
		BlockVectorReturn<Lhs, Dest> tmpDest(m_lhs, dest);
		internal::sparse_time_dense_product(
			BlockSparseMatrixView<Lhs>(m_lhs), BlockVectorView<Lhs, Rhs>(m_lhs, m_rhs), tmpDest, alpha);
	}

  private:
	BlockSparseTimeDenseProduct& operator=(const BlockSparseTimeDenseProduct&);
};

template<typename _Scalar, int _BlockAtCompileTime, int _Options, typename _StorageIndex>
class BlockSparseMatrix
	: public SparseMatrixBase<BlockSparseMatrix<_Scalar, _BlockAtCompileTime, _Options, _StorageIndex>>
{
  public:
	typedef _Scalar Scalar;
	typedef typename NumTraits<Scalar>::Real RealScalar;
	typedef _StorageIndex StorageIndex;
	typedef
		typename internal::ref_selector<BlockSparseMatrix<_Scalar, _BlockAtCompileTime, _Options, _StorageIndex>>::type
			Nested;

	enum
	{
		Options = _Options,
		Flags = Options,
		BlockSize = _BlockAtCompileTime,
		RowsAtCompileTime = Dynamic,
		ColsAtCompileTime = Dynamic,
		MaxRowsAtCompileTime = Dynamic,
		MaxColsAtCompileTime = Dynamic,
		IsVectorAtCompileTime = 0,
		IsColMajor = Flags & RowMajorBit ? 0 : 1
	};
	typedef Matrix<Scalar, _BlockAtCompileTime, _BlockAtCompileTime, IsColMajor ? ColMajor : RowMajor> BlockScalar;
	typedef Matrix<RealScalar, _BlockAtCompileTime, _BlockAtCompileTime, IsColMajor ? ColMajor : RowMajor>
		BlockRealScalar;
	typedef
		typename internal::conditional<_BlockAtCompileTime == Dynamic, Scalar, BlockScalar>::type BlockScalarReturnType;
	typedef BlockSparseMatrix<Scalar, BlockSize, IsColMajor ? ColMajor : RowMajor, StorageIndex> PlainObject;

  public:
	// Default constructor
	BlockSparseMatrix()
		: m_innerBSize(0)
		, m_outerBSize(0)
		, m_innerOffset(0)
		, m_outerOffset(0)
		, m_nonzerosblocks(0)
		, m_values(0)
		, m_blockPtr(0)
		, m_indices(0)
		, m_outerIndex(0)
		, m_blockSize(BlockSize)
	{
	}

	/**
	 * \brief Construct and resize
	 *
	 */
	BlockSparseMatrix(Index brow, Index bcol)
		: m_innerBSize(IsColMajor ? brow : bcol)
		, m_outerBSize(IsColMajor ? bcol : brow)
		, m_innerOffset(0)
		, m_outerOffset(0)
		, m_nonzerosblocks(0)
		, m_values(0)
		, m_blockPtr(0)
		, m_indices(0)
		, m_outerIndex(0)
		, m_blockSize(BlockSize)
	{
	}

	/**
	 * \brief Copy-constructor
	 */
	BlockSparseMatrix(const BlockSparseMatrix& other)
		: m_innerBSize(other.m_innerBSize)
		, m_outerBSize(other.m_outerBSize)
		, m_nonzerosblocks(other.m_nonzerosblocks)
		, m_nonzeros(other.m_nonzeros)
		, m_blockPtr(0)
		, m_blockSize(other.m_blockSize)
	{
		// should we allow copying between variable-size blocks and fixed-size blocks ??
		eigen_assert(m_blockSize == BlockSize && " CAN NOT COPY BETWEEN FIXED-SIZE AND VARIABLE-SIZE BLOCKS");

		std::copy(other.m_innerOffset, other.m_innerOffset + m_innerBSize + 1, m_innerOffset);
		std::copy(other.m_outerOffset, other.m_outerOffset + m_outerBSize + 1, m_outerOffset);
		std::copy(other.m_values, other.m_values + m_nonzeros, m_values);

		if (m_blockSize != Dynamic)
			std::copy(other.m_blockPtr, other.m_blockPtr + m_nonzerosblocks, m_blockPtr);

		std::copy(other.m_indices, other.m_indices + m_nonzerosblocks, m_indices);
		std::copy(other.m_outerIndex, other.m_outerIndex + m_outerBSize, m_outerIndex);
	}

	friend void swap(BlockSparseMatrix& first, BlockSparseMatrix& second)
	{
		std::swap(first.m_innerBSize, second.m_innerBSize);
		std::swap(first.m_outerBSize, second.m_outerBSize);
		std::swap(first.m_innerOffset, second.m_innerOffset);
		std::swap(first.m_outerOffset, second.m_outerOffset);
		std::swap(first.m_nonzerosblocks, second.m_nonzerosblocks);
		std::swap(first.m_nonzeros, second.m_nonzeros);
		std::swap(first.m_values, second.m_values);
		std::swap(first.m_blockPtr, second.m_blockPtr);
		std::swap(first.m_indices, second.m_indices);
		std::swap(first.m_outerIndex, second.m_outerIndex);
		std::swap(first.m_BlockSize, second.m_blockSize);
	}

	BlockSparseMatrix& operator=(BlockSparseMatrix other)
	{
		// Copy-and-swap paradigm ... avoid leaked data if thrown
		swap(*this, other);
		return *this;
	}

	// Destructor
	~BlockSparseMatrix()
	{
		delete[] m_outerIndex;
		delete[] m_innerOffset;
		delete[] m_outerOffset;
		delete[] m_indices;
		delete[] m_blockPtr;
		delete[] m_values;
	}

	/**
	 * \brief Constructor from a sparse matrix
	 *
	 */
	template<typename MatrixType>
	inline BlockSparseMatrix(const MatrixType& spmat)
		: m_blockSize(BlockSize)
	{
		EIGEN_STATIC_ASSERT((m_blockSize != Dynamic), THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE);

		*this = spmat;
	}

	/**
	 * \brief Assignment from a sparse matrix with the same storage order
	 *
	 * Convert from a sparse matrix to block sparse matrix.
	 * \warning Before calling this function, tt is necessary to call
	 * either setBlockLayout() (matrices with variable-size blocks)
	 * or setBlockSize() (for fixed-size blocks).
	 */
	template<typename MatrixType>
	inline BlockSparseMatrix& operator=(const MatrixType& spmat)
	{
		eigen_assert((m_innerBSize != 0 && m_outerBSize != 0) &&
					 "Trying to assign to a zero-size matrix, call resize() first");
		eigen_assert(((MatrixType::Options & RowMajorBit) != IsColMajor) && "Wrong storage order");
		typedef SparseMatrix<bool, MatrixType::Options, typename MatrixType::Index> MatrixPatternType;
		MatrixPatternType blockPattern(blockRows(), blockCols());
		m_nonzeros = 0;

		// First, compute the number of nonzero blocks and their locations
		for (StorageIndex bj = 0; bj < m_outerBSize; ++bj) {
			// Browse each outer block and compute the structure
			std::vector<bool> nzblocksFlag(m_innerBSize, false); // Record the existing blocks
			blockPattern.startVec(bj);
			for (StorageIndex j = blockOuterIndex(bj); j < blockOuterIndex(bj + 1); ++j) {
				typename MatrixType::InnerIterator it_spmat(spmat, j);
				for (; it_spmat; ++it_spmat) {
					StorageIndex bi = innerToBlock(it_spmat.index()); // Index of the current nonzero block
					if (!nzblocksFlag[bi]) {
						// Save the index of this nonzero block
						nzblocksFlag[bi] = true;
						blockPattern.insertBackByOuterInnerUnordered(bj, bi) = true;
						// Compute the total number of nonzeros (including explicit zeros in blocks)
						m_nonzeros += blockOuterSize(bj) * blockInnerSize(bi);
					}
				}
			} // end current outer block
		}
		blockPattern.finalize();

		// Allocate the internal arrays
		setBlockStructure(blockPattern);

		for (StorageIndex nz = 0; nz < m_nonzeros; ++nz)
			m_values[nz] = Scalar(0);
		for (StorageIndex bj = 0; bj < m_outerBSize; ++bj) {
			// Now copy the values
			for (StorageIndex j = blockOuterIndex(bj); j < blockOuterIndex(bj + 1); ++j) {
				// Browse the outer block column by column (for column-major matrices)
				typename MatrixType::InnerIterator it_spmat(spmat, j);
				for (; it_spmat; ++it_spmat) {
					StorageIndex idx = 0;							  // Position of this block in the column block
					StorageIndex bi = innerToBlock(it_spmat.index()); // Index of the current nonzero block
					// Go to the inner block where this element belongs to
					while (bi > m_indices[m_outerIndex[bj] + idx])
						++idx;			 // Not expensive for ordered blocks
					StorageIndex idxVal; // Get the right position in the array of values for this element
					if (m_blockSize == Dynamic) {
						// Offset from all blocks before ...
						idxVal = m_blockPtr[m_outerIndex[bj] + idx];
						// ... and offset inside the block
						idxVal += (j - blockOuterIndex(bj)) * blockOuterSize(bj) + it_spmat.index() - m_innerOffset[bi];
					} else {
						// All blocks before
						idxVal = (m_outerIndex[bj] + idx) * m_blockSize * m_blockSize;
						// inside the block
						idxVal += (j - blockOuterIndex(bj)) * m_blockSize + (it_spmat.index() % m_blockSize);
					}
					// Insert the value
					m_values[idxVal] = it_spmat.value();
				} // end of this column
			}	  // end of this block
		}		  // end of this outer block

		return *this;
	}

	/**
	 * \brief Set the nonzero block pattern of the matrix
	 *
	 * Given a sparse matrix describing the nonzero block pattern,
	 * this function prepares the internal pointers for values.
	 * After calling this function, any *nonzero* block (bi, bj) can be set
	 * with a simple call to coeffRef(bi,bj).
	 *
	 *
	 * \warning Before calling this function, tt is necessary to call
	 * either setBlockLayout() (matrices with variable-size blocks)
	 * or setBlockSize() (for fixed-size blocks).
	 *
	 * \param blockPattern Sparse matrix of boolean elements describing the block structure
	 *
	 * \sa setBlockLayout() \sa setBlockSize()
	 */
	template<typename MatrixType>
	void setBlockStructure(const MatrixType& blockPattern)
	{
		resize(blockPattern.rows(), blockPattern.cols());
		reserve(blockPattern.nonZeros());

		// Browse the block pattern and set up the various pointers
		m_outerIndex[0] = 0;
		if (m_blockSize == Dynamic)
			m_blockPtr[0] = 0;
		for (StorageIndex nz = 0; nz < m_nonzeros; ++nz)
			m_values[nz] = Scalar(0);
		for (StorageIndex bj = 0; bj < m_outerBSize; ++bj) {
			// Browse each outer block

			// First, copy and save the indices of nonzero blocks
			// FIXME : find a way to avoid this ...
			std::vector<int> nzBlockIdx;
			typename MatrixType::InnerIterator it(blockPattern, bj);
			for (; it; ++it) {
				nzBlockIdx.push_back(it.index());
			}
			std::sort(nzBlockIdx.begin(), nzBlockIdx.end());

			// Now, fill block indices and (eventually) pointers to blocks
			for (StorageIndex idx = 0; idx < nzBlockIdx.size(); ++idx) {
				StorageIndex offset = m_outerIndex[bj] + idx; // offset in m_indices
				m_indices[offset] = nzBlockIdx[idx];
				if (m_blockSize == Dynamic)
					m_blockPtr[offset] = m_blockPtr[offset - 1] + blockInnerSize(nzBlockIdx[idx]) * blockOuterSize(bj);
				// There is no blockPtr for fixed-size blocks... not needed !???
			}
			// Save the pointer to the next outer block
			m_outerIndex[bj + 1] = m_outerIndex[bj] + nzBlockIdx.size();
		}
	}

	/**
	 * \brief Set the number of rows and columns blocks
	 */
	inline void resize(Index brow, Index bcol)
	{
		m_innerBSize = IsColMajor ? brow : bcol;
		m_outerBSize = IsColMajor ? bcol : brow;
	}

	/**
	 * \brief set the block size at runtime for fixed-size block layout
	 *
	 * Call this only for fixed-size blocks
	 */
	inline void setBlockSize(Index blockSize) { m_blockSize = blockSize; }

	/**
	 * \brief Set the row and column block layouts,
	 *
	 * This function set the size of each row and column block.
	 * So this function should be used only for blocks with variable size.
	 * \param rowBlocks : Number of rows per row block
	 * \param colBlocks : Number of columns per column block
	 * \sa resize(), setBlockSize()
	 */
	inline void setBlockLayout(const VectorXi& rowBlocks, const VectorXi& colBlocks)
	{
		const VectorXi& innerBlocks = IsColMajor ? rowBlocks : colBlocks;
		const VectorXi& outerBlocks = IsColMajor ? colBlocks : rowBlocks;
		eigen_assert(m_innerBSize == innerBlocks.size() && "CHECK THE NUMBER OF ROW OR COLUMN BLOCKS");
		eigen_assert(m_outerBSize == outerBlocks.size() && "CHECK THE NUMBER OF ROW OR COLUMN BLOCKS");
		m_outerBSize = outerBlocks.size();
		//  starting index of blocks... cumulative sums
		m_innerOffset = new StorageIndex[m_innerBSize + 1];
		m_outerOffset = new StorageIndex[m_outerBSize + 1];
		m_innerOffset[0] = 0;
		m_outerOffset[0] = 0;
		std::partial_sum(&innerBlocks[0], &innerBlocks[m_innerBSize - 1] + 1, &m_innerOffset[1]);
		std::partial_sum(&outerBlocks[0], &outerBlocks[m_outerBSize - 1] + 1, &m_outerOffset[1]);

		// Compute the total number of nonzeros
		m_nonzeros = 0;
		for (StorageIndex bj = 0; bj < m_outerBSize; ++bj)
			for (StorageIndex bi = 0; bi < m_innerBSize; ++bi)
				m_nonzeros += outerBlocks[bj] * innerBlocks[bi];
	}

	/**
	 * \brief Allocate the internal array of pointers to blocks and their inner indices
	 *
	 * \note For fixed-size blocks, call setBlockSize() to set the block.
	 * And For variable-size blocks, call setBlockLayout() before using this function
	 *
	 * \param nonzerosblocks Number of nonzero blocks. The total number of nonzeros is
	 * is computed in setBlockLayout() for variable-size blocks
	 * \sa setBlockSize()
	 */
	inline void reserve(const Index nonzerosblocks)
	{
		eigen_assert((m_innerBSize != 0 && m_outerBSize != 0) &&
					 "TRYING TO RESERVE ZERO-SIZE MATRICES, CALL resize() first");

		// FIXME Should free if already allocated
		m_outerIndex = new StorageIndex[m_outerBSize + 1];

		m_nonzerosblocks = nonzerosblocks;
		if (m_blockSize != Dynamic) {
			m_nonzeros = nonzerosblocks * (m_blockSize * m_blockSize);
			m_blockPtr = 0;
		} else {
			// m_nonzeros  is already computed in setBlockLayout()
			m_blockPtr = new StorageIndex[m_nonzerosblocks + 1];
		}
		m_indices = new StorageIndex[m_nonzerosblocks + 1];
		m_values = new Scalar[m_nonzeros];
	}

	/**
	 * \brief Fill values in a matrix  from a triplet list.
	 *
	 * Each triplet item has a block stored in an Eigen dense matrix.
	 * The InputIterator class should provide the functions row(), col() and value()
	 *
	 * \note For fixed-size blocks, call setBlockSize() before this function.
	 *
	 * FIXME Do not accept duplicates
	 */
	template<typename InputIterator>
	void setFromTriplets(const InputIterator& begin, const InputIterator& end)
	{
		eigen_assert((m_innerBSize != 0 && m_outerBSize != 0) && "ZERO BLOCKS, PLEASE CALL resize() before");

		/* First, sort the triplet list
		 * FIXME This can be unnecessarily expensive since only the inner indices have to be sorted
		 * The best approach is like in SparseMatrix::setFromTriplets()
		 */
		internal::TripletComp<InputIterator, IsColMajor> tripletcomp;
		std::sort(begin, end, tripletcomp);

		/* Count the number of rows and column blocks,
		 * and the number of nonzero blocks per outer dimension
		 */
		VectorXi rowBlocks(m_innerBSize); // Size of each block row
		VectorXi colBlocks(m_outerBSize); // Size of each block column
		rowBlocks.setZero();
		colBlocks.setZero();
		VectorXi nzblock_outer(m_outerBSize); // Number of nz blocks per outer vector
		VectorXi nz_outer(m_outerBSize);	  // Number of nz per outer vector...for variable-size blocks
		nzblock_outer.setZero();
		nz_outer.setZero();
		for (InputIterator it(begin); it != end; ++it) {
			eigen_assert(it->row() >= 0 && it->row() < this->blockRows() && it->col() >= 0 &&
						 it->col() < this->blockCols());
			eigen_assert((it->value().rows() == it->value().cols() && (it->value().rows() == m_blockSize)) ||
						 (m_blockSize == Dynamic));

			if (m_blockSize == Dynamic) {
				eigen_assert((rowBlocks[it->row()] == 0 || rowBlocks[it->row()] == it->value().rows()) &&
							 "NON CORRESPONDING SIZES FOR ROW BLOCKS");
				eigen_assert((colBlocks[it->col()] == 0 || colBlocks[it->col()] == it->value().cols()) &&
							 "NON CORRESPONDING SIZES FOR COLUMN BLOCKS");
				rowBlocks[it->row()] = it->value().rows();
				colBlocks[it->col()] = it->value().cols();
			}
			nz_outer(IsColMajor ? it->col() : it->row()) += it->value().rows() * it->value().cols();
			nzblock_outer(IsColMajor ? it->col() : it->row())++;
		}
		// Allocate member arrays
		if (m_blockSize == Dynamic)
			setBlockLayout(rowBlocks, colBlocks);
		StorageIndex nzblocks = nzblock_outer.sum();
		reserve(nzblocks);

		// Temporary markers
		VectorXi block_id(m_outerBSize); // To be used as a block marker during insertion

		// Setup outer index pointers and markers
		m_outerIndex[0] = 0;
		if (m_blockSize == Dynamic)
			m_blockPtr[0] = 0;
		for (StorageIndex bj = 0; bj < m_outerBSize; ++bj) {
			m_outerIndex[bj + 1] = m_outerIndex[bj] + nzblock_outer(bj);
			block_id(bj) = m_outerIndex[bj];
			if (m_blockSize == Dynamic) {
				m_blockPtr[m_outerIndex[bj + 1]] = m_blockPtr[m_outerIndex[bj]] + nz_outer(bj);
			}
		}

		// Fill the matrix
		for (InputIterator it(begin); it != end; ++it) {
			StorageIndex outer = IsColMajor ? it->col() : it->row();
			StorageIndex inner = IsColMajor ? it->row() : it->col();
			m_indices[block_id(outer)] = inner;
			StorageIndex block_size = it->value().rows() * it->value().cols();
			StorageIndex nz_marker = blockPtr(block_id[outer]);
			memcpy(&(m_values[nz_marker]), it->value().data(), block_size * sizeof(Scalar));
			if (m_blockSize == Dynamic) {
				m_blockPtr[block_id(outer) + 1] = m_blockPtr[block_id(outer)] + block_size;
			}
			block_id(outer)++;
		}

		// An alternative when the outer indices are sorted...no need to use an array of markers
		//      for(Index bcol = 0; bcol < m_outerBSize; ++bcol)
		//      {
		//      Index id = 0, id_nz = 0, id_nzblock = 0;
		//      for(InputIterator it(begin); it!=end; ++it)
		//      {
		//        while (id<bcol) // one pass should do the job unless there are empty columns
		//        {
		//          id++;
		//          m_outerIndex[id+1]=m_outerIndex[id];
		//        }
		//        m_outerIndex[id+1] += 1;
		//        m_indices[id_nzblock]=brow;
		//        Index block_size = it->value().rows()*it->value().cols();
		//        m_blockPtr[id_nzblock+1] = m_blockPtr[id_nzblock] + block_size;
		//        id_nzblock++;
		//        memcpy(&(m_values[id_nz]),it->value().data(), block_size*sizeof(Scalar));
		//        id_nz += block_size;
		//      }
		//      while(id < m_outerBSize-1) // Empty columns at the end
		//      {
		//        id++;
		//        m_outerIndex[id+1]=m_outerIndex[id];
		//      }
		//      }
	}

	/**
	 * \returns the number of rows
	 */
	inline Index rows() const
	{
		//      return blockRows();
		return (IsColMajor ? innerSize() : outerSize());
	}

	/**
	 * \returns the number of cols
	 */
	inline Index cols() const
	{
		//      return blockCols();
		return (IsColMajor ? outerSize() : innerSize());
	}

	inline Index innerSize() const
	{
		if (m_blockSize == Dynamic)
			return m_innerOffset[m_innerBSize];
		else
			return (m_innerBSize * m_blockSize);
	}

	inline Index outerSize() const
	{
		if (m_blockSize == Dynamic)
			return m_outerOffset[m_outerBSize];
		else
			return (m_outerBSize * m_blockSize);
	}
	/** \returns the number of rows grouped by blocks */
	inline Index blockRows() const { return (IsColMajor ? m_innerBSize : m_outerBSize); }
	/** \returns the number of columns grouped by blocks */
	inline Index blockCols() const { return (IsColMajor ? m_outerBSize : m_innerBSize); }

	inline Index outerBlocks() const { return m_outerBSize; }
	inline Index innerBlocks() const { return m_innerBSize; }

	/** \returns the block index where outer belongs to */
	inline Index outerToBlock(Index outer) const
	{
		eigen_assert(outer < outerSize() && "OUTER INDEX OUT OF BOUNDS");

		if (m_blockSize != Dynamic)
			return (outer / m_blockSize); // Integer division

		StorageIndex b_outer = 0;
		while (m_outerOffset[b_outer] <= outer)
			++b_outer;
		return b_outer - 1;
	}
	/** \returns  the block index where inner belongs to */
	inline Index innerToBlock(Index inner) const
	{
		eigen_assert(inner < innerSize() && "OUTER INDEX OUT OF BOUNDS");

		if (m_blockSize != Dynamic)
			return (inner / m_blockSize); // Integer division

		StorageIndex b_inner = 0;
		while (m_innerOffset[b_inner] <= inner)
			++b_inner;
		return b_inner - 1;
	}

	/**
	 *\returns a reference to the (i,j) block as an Eigen Dense Matrix
	 */
	Ref<BlockScalar> coeffRef(Index brow, Index bcol)
	{
		eigen_assert(brow < blockRows() && "BLOCK ROW INDEX OUT OF BOUNDS");
		eigen_assert(bcol < blockCols() && "BLOCK nzblocksFlagCOLUMN OUT OF BOUNDS");

		StorageIndex rsize = IsColMajor ? blockInnerSize(brow) : blockOuterSize(bcol);
		StorageIndex csize = IsColMajor ? blockOuterSize(bcol) : blockInnerSize(brow);
		StorageIndex inner = IsColMajor ? brow : bcol;
		StorageIndex outer = IsColMajor ? bcol : brow;
		StorageIndex offset = m_outerIndex[outer];
		while (offset < m_outerIndex[outer + 1] && m_indices[offset] != inner)
			offset++;
		if (m_indices[offset] == inner) {
			return Map<BlockScalar>(&(m_values[blockPtr(offset)]), rsize, csize);
		} else {
			// FIXME the block does not exist, Insert it !!!!!!!!!
			eigen_assert("DYNAMIC INSERTION IS NOT YET SUPPORTED");
		}
	}

	/**
	 * \returns the value of the (i,j) block as an Eigen Dense Matrix
	 */
	Map<const BlockScalar> coeff(Index brow, Index bcol) const
	{
		eigen_assert(brow < blockRows() && "BLOCK ROW INDEX OUT OF BOUNDS");
		eigen_assert(bcol < blockCols() && "BLOCK COLUMN OUT OF BOUNDS");

		StorageIndex rsize = IsColMajor ? blockInnerSize(brow) : blockOuterSize(bcol);
		StorageIndex csize = IsColMajor ? blockOuterSize(bcol) : blockInnerSize(brow);
		StorageIndex inner = IsColMajor ? brow : bcol;
		StorageIndex outer = IsColMajor ? bcol : brow;
		StorageIndex offset = m_outerIndex[outer];
		while (offset < m_outerIndex[outer + 1] && m_indices[offset] != inner)
			offset++;
		if (m_indices[offset] == inner) {
			return Map<const BlockScalar>(&(m_values[blockPtr(offset)]), rsize, csize);
		} else
			//        return BlockScalar::Zero(rsize, csize);
			eigen_assert("NOT YET SUPPORTED");
	}

	// Block Matrix times vector product
	template<typename VecType>
	BlockSparseTimeDenseProduct<BlockSparseMatrix, VecType> operator*(const VecType& lhs) const
	{
		return BlockSparseTimeDenseProduct<BlockSparseMatrix, VecType>(*this, lhs);
	}

	/** \returns the number of nonzero blocks */
	inline Index nonZerosBlocks() const { return m_nonzerosblocks; }
	/** \returns the total number of nonzero elements, including eventual explicit zeros in blocks */
	inline Index nonZeros() const { return m_nonzeros; }

	inline BlockScalarReturnType* valuePtr() { return static_cast<BlockScalarReturnType*>(m_values); }
	//    inline Scalar *valuePtr(){ return m_values; }
	inline StorageIndex* innerIndexPtr() { return m_indices; }
	inline const StorageIndex* innerIndexPtr() const { return m_indices; }
	inline StorageIndex* outerIndexPtr() { return m_outerIndex; }
	inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }

	/** \brief for compatibility purposes with the SparseMatrix class */
	inline bool isCompressed() const { return true; }
	/**
	 * \returns the starting index of the bi row block
	 */
	inline Index blockRowsIndex(Index bi) const { return IsColMajor ? blockInnerIndex(bi) : blockOuterIndex(bi); }

	/**
	 * \returns the starting index of the bj col block
	 */
	inline Index blockColsIndex(Index bj) const { return IsColMajor ? blockOuterIndex(bj) : blockInnerIndex(bj); }

	inline Index blockOuterIndex(Index bj) const
	{
		return (m_blockSize == Dynamic) ? m_outerOffset[bj] : (bj * m_blockSize);
	}
	inline Index blockInnerIndex(Index bi) const
	{
		return (m_blockSize == Dynamic) ? m_innerOffset[bi] : (bi * m_blockSize);
	}

	// Not needed ???
	inline Index blockInnerSize(Index bi) const
	{
		return (m_blockSize == Dynamic) ? (m_innerOffset[bi + 1] - m_innerOffset[bi]) : m_blockSize;
	}
	inline Index blockOuterSize(Index bj) const
	{
		return (m_blockSize == Dynamic) ? (m_outerOffset[bj + 1] - m_outerOffset[bj]) : m_blockSize;
	}

	/**
	 * \brief Browse the matrix by outer index
	 */
	class InnerIterator; // Browse column by column

	/**
	 * \brief Browse the matrix by block outer index
	 */
	class BlockInnerIterator; // Browse block by block

	friend std::ostream& operator<<(std::ostream& s, const BlockSparseMatrix& m)
	{
		for (StorageIndex j = 0; j < m.outerBlocks(); ++j) {
			BlockInnerIterator itb(m, j);
			for (; itb; ++itb) {
				s << "(" << itb.row() << ", " << itb.col() << ")\n";
				s << itb.value() << "\n";
			}
		}
		s << std::endl;
		return s;
	}

	/**
	 * \returns the starting position of the block \p id in the array of values
	 */
	Index blockPtr(Index id) const
	{
		if (m_blockSize == Dynamic)
			return m_blockPtr[id];
		else
			return id * m_blockSize * m_blockSize;
		// return blockDynIdx(id, typename internal::conditional<(BlockSize==Dynamic), internal::true_type,
		// internal::false_type>::type());
	}

  protected:
	//    inline Index blockDynIdx(Index id, internal::true_type) const
	//    {
	//      return m_blockPtr[id];
	//    }
	//    inline Index blockDynIdx(Index id, internal::false_type) const
	//    {
	//      return id * BlockSize * BlockSize;
	//    }

	// To be implemented
	// Insert a block at a particular location... need to make a room for that
	Map<BlockScalar> insert(Index brow, Index bcol);

	Index m_innerBSize;			 // Number of block rows
	Index m_outerBSize;			 // Number of block columns
	StorageIndex* m_innerOffset; // Starting index of each inner block (size m_innerBSize+1)
	StorageIndex* m_outerOffset; // Starting index of each outer block (size m_outerBSize+1)
	Index m_nonzerosblocks;		 // Total nonzeros blocks (lower than  m_innerBSize x m_outerBSize)
	Index m_nonzeros;			 // Total nonzeros elements
	Scalar* m_values;			 // Values stored block column after block column (size m_nonzeros)
	StorageIndex* m_blockPtr;	// Pointer to the beginning of each block in m_values, size m_nonzeroblocks ... null for
								// fixed-size blocks
	StorageIndex* m_indices;	// Inner block indices, size m_nonzerosblocks ... OK
	StorageIndex* m_outerIndex; // Starting pointer of each block column in m_indices (size m_outerBSize)... OK
	Index m_blockSize;			// Size of a block for fixed-size blocks, otherwise -1
};

template<typename _Scalar, int _BlockAtCompileTime, int _Options, typename _StorageIndex>
class BlockSparseMatrix<_Scalar, _BlockAtCompileTime, _Options, _StorageIndex>::BlockInnerIterator
{
  public:
	enum
	{
		Flags = _Options
	};

	BlockInnerIterator(const BlockSparseMatrix& mat, const Index outer)
		: m_mat(mat)
		, m_outer(outer)
		, m_id(mat.m_outerIndex[outer])
		, m_end(mat.m_outerIndex[outer + 1])
	{
	}

	inline BlockInnerIterator& operator++()
	{
		m_id++;
		return *this;
	}

	inline const Map<const BlockScalar> value() const
	{
		return Map<const BlockScalar>(&(m_mat.m_values[m_mat.blockPtr(m_id)]), rows(), cols());
	}
	inline Map<BlockScalar> valueRef()
	{
		return Map<BlockScalar>(&(m_mat.m_values[m_mat.blockPtr(m_id)]), rows(), cols());
	}
	// Block inner index
	inline Index index() const { return m_mat.m_indices[m_id]; }
	inline Index outer() const { return m_outer; }
	// block row index
	inline Index row() const { return index(); }
	// block column index
	inline Index col() const { return outer(); }
	// FIXME Number of rows in the current block
	inline Index rows() const
	{
		return (m_mat.m_blockSize == Dynamic) ? (m_mat.m_innerOffset[index() + 1] - m_mat.m_innerOffset[index()])
											  : m_mat.m_blockSize;
	}
	// Number of columns in the current block ...
	inline Index cols() const
	{
		return (m_mat.m_blockSize == Dynamic) ? (m_mat.m_outerOffset[m_outer + 1] - m_mat.m_outerOffset[m_outer])
											  : m_mat.m_blockSize;
	}
	inline operator bool() const { return (m_id < m_end); }

  protected:
	const BlockSparseMatrix<_Scalar, _BlockAtCompileTime, _Options, StorageIndex>& m_mat;
	const Index m_outer;
	Index m_id;
	Index m_end;
};

template<typename _Scalar, int _BlockAtCompileTime, int _Options, typename _StorageIndex>
class BlockSparseMatrix<_Scalar, _BlockAtCompileTime, _Options, _StorageIndex>::InnerIterator
{
  public:
	InnerIterator(const BlockSparseMatrix& mat, Index outer)
		: m_mat(mat)
		, m_outerB(mat.outerToBlock(outer))
		, m_outer(outer)
		, itb(mat, mat.outerToBlock(outer))
		, m_offset(outer - mat.blockOuterIndex(m_outerB))
	{
		if (itb) {
			m_id = m_mat.blockInnerIndex(itb.index());
			m_start = m_id;
			m_end = m_mat.blockInnerIndex(itb.index() + 1);
		}
	}
	inline InnerIterator& operator++()
	{
		m_id++;
		if (m_id >= m_end) {
			++itb;
			if (itb) {
				m_id = m_mat.blockInnerIndex(itb.index());
				m_start = m_id;
				m_end = m_mat.blockInnerIndex(itb.index() + 1);
			}
		}
		return *this;
	}
	inline const Scalar& value() const { return itb.value().coeff(m_id - m_start, m_offset); }
	inline Scalar& valueRef() { return itb.valueRef().coeff(m_id - m_start, m_offset); }
	inline Index index() const { return m_id; }
	inline Index outer() const { return m_outer; }
	inline Index col() const { return outer(); }
	inline Index row() const { return index(); }
	inline operator bool() const { return itb; }

  protected:
	const BlockSparseMatrix& m_mat;
	const Index m_outer;
	const Index m_outerB;
	BlockInnerIterator itb; // Iterator through the blocks
	const Index m_offset;	// Position of this column in the block
	Index m_start;			// starting inner index of this block
	Index m_id;				// current inner index in the block
	Index m_end;			// starting inner index of the next block
};
} // end namespace Eigen

#endif // EIGEN_SPARSEBLOCKMATRIX_H
